#include "preprocess/voxel.h"
namespace autodrive {
namespace perception {
Voxel::Voxel() noexcept {}

Voxel::~Voxel() {}

bool Voxel::init(VoxelParameters const& para, uint32_t pointcloud_number,
                 std::string& err) noexcept {
  auto bRet = true;
  auto initFunc{[&]() {
    auto const& voxelSize{para.voxel_size};
    auto const& netRange{para.net_range};
    // 参数合法检查
    if (para.mp.size() < 2) {
      err = "parameter mp size illegal, size= " + para.mp.size();
      bRet = false;
      return;
    }

    if (para.voxel_size.size() < 3) {
      err = "parameter voxel size illegal, size= " + para.voxel_size.size();
      bRet = false;
      return;
    }

    if (para.net_range.size() < 6) {
      err = "parameter net range  illegal, size= " + para.net_range.size();
      bRet = false;
      return;
    }

    for (auto const& size : voxelSize) {
      if (size == 0) {
        err = "voxelSize cannot be 0";
        bRet = false;
        return;
      }
    }

    // 资源分配
    grid_size_.resize(voxelSize.size());

    for (std::uint16_t i{0U}; i < grid_size_.size(); ++i) {
      grid_size_[i] =
          round((netRange[grid_size_.size() + i] - netRange[i]) / voxelSize[i]);
    }

    auto const& gridTotal{std::accumulate(grid_size_.begin(), grid_size_.end(),
                                          1, std::multiplies<uint32_t>())};
    voxel_points_num_lut_ptr_ =
        std::make_shared<std::vector<std::atomic_uint8_t>>(gridTotal);

    random_array_ptr_ = std::make_shared<std::vector<uint32_t>>();

    bRet = true;
  }};

  std::call_once(init_flag_, initFunc);

  if (pointcloud_number > random_array_ptr_->size()) {
    // 生成随机数组
    random_array_ptr_->resize(pointcloud_number);
    for (std::uint32_t i{0}; i < pointcloud_number; ++i) {
      random_array_ptr_->at(i) = i;
    }
    if (para.is_shuffle) {
      std::shuffle(
          random_array_ptr_->begin(), random_array_ptr_->end(),
          std::default_random_engine(
              std::chrono::system_clock::now().time_since_epoch().count()));
    }
  }

  return bRet;
}

/// @brief
void Voxel::Reset() noexcept {
  if (voxel_points_num_lut_ptr_ != nullptr &&
      voxel_points_num_lut_ptr_.get() != nullptr) {
    for (auto& p : *voxel_points_num_lut_ptr_.get()) {
      p.store(0);
    }
  }
}
}  // namespace perception
}  // namespace autodrive